// ball_motion_analysis.cpp
#include "ball_motion_analysis.h"
#include <cmath>
#include <algorithm>

BallMotionAnalyzer::BallMotionAnalyzer(int max_history, 
                                     float speed_thresh,
                                     float accel_thresh,
                                     float dir_change_thresh)
    : max_history_frames_(max_history)
    , speed_threshold_(speed_thresh)
    , acceleration_threshold_(accel_thresh)
    , direction_change_threshold_(dir_change_thresh) {
}

void BallMotionAnalyzer::addMotionData(int frame_idx, const ball_info_t& ball_info) {
    if (!ball_info.avail) {
        return;
    }
    
    ball_motion_t motion = calculateMotion(frame_idx, ball_info);
    
    // 添加到历史记录
    motion_history_.push_back(motion);
    
    // 保持历史记录长度
    if (motion_history_.size() > max_history_frames_) {
        motion_history_.pop_front();
    }
}

ball_motion_t BallMotionAnalyzer::calculateMotion(int frame_idx, const ball_info_t& ball_info) {
    ball_motion_t motion;
    motion.frame_idx = frame_idx;
    motion.position = ball_info.court_relative_pos;
    motion.valid = ball_info.avail;
    
    // 如果没有足够的历史数据，无法计算速度和加速度
    if (motion_history_.size() < 2) {
        motion.velocity = {0, 0, 0};
        motion.speed = 0;
        motion.acceleration = 0;
        return motion;
    }
    
    // 计算速度（使用最近两帧）
    const ball_motion_t& prev_motion = motion_history_.back();
    //const ball_motion_t& prev_prev_motion = *(motion_history_.rbegin() + 1);
    
    // 假设帧率30fps，时间间隔1/30秒
    float dt = 1.0f / 30.0f;
    
    // 当前速度
    motion.velocity.x = (motion.position.x - prev_motion.position.x) / dt;
    motion.velocity.y = (motion.position.y - prev_motion.position.y) / dt;
    motion.velocity.z = (motion.position.z - prev_motion.position.z) / dt;
    
    motion.speed = sqrt(motion.velocity.x * motion.velocity.x + 
                       motion.velocity.y * motion.velocity.y + 
                       motion.velocity.z * motion.velocity.z);
    
    // 计算加速度
    float prev_speed = prev_motion.speed;
    motion.acceleration = (motion.speed - prev_speed) / dt;
    
    return motion;
}

com_point_t BallMotionAnalyzer::smoothPosition(const com_point_t& current_pos) {
    if (motion_history_.size() < 3) {
        return current_pos;
    }
    
    // 简单移动平均滤波
    com_point_t smoothed = {0, 0, 0};
    int count = 0;
    
    for (const auto& motion : motion_history_) {
        smoothed.x += motion.position.x;
        smoothed.y += motion.position.y;
        smoothed.z += motion.position.z;
        count++;
    }
    
    smoothed.x /= count;
    smoothed.y /= count;
    smoothed.z /= count;
    
    return smoothed;
}

std::vector<hit_point_t> BallMotionAnalyzer::detectHitPoints() {
    std::vector<hit_point_t> hit_points;
    
    if (motion_history_.size() < 5) {
        return hit_points;
    }
    
    for (size_t i = 2; i < motion_history_.size() - 2; i++) {
        const ball_motion_t& prev2 = motion_history_[i-2];
        const ball_motion_t& prev1 = motion_history_[i-1];
        const ball_motion_t& current = motion_history_[i];
        const ball_motion_t& next1 = motion_history_[i+1];
        const ball_motion_t& next2 = motion_history_[i+2];
        
        if (!prev2.valid || !prev1.valid || !current.valid || !next1.valid || !next2.valid) {
            continue;
        }
        
        // 检查速度突变
        float speed_before = (prev2.speed + prev1.speed) / 2.0f;
        float speed_after = (next1.speed + next2.speed) / 2.0f;
        float speed_change = fabs(speed_after - speed_before);
        
        // 检查方向变化
        com_point_t dir_before = {
            (prev1.position.x - prev2.position.x),
            (prev1.position.y - prev2.position.y),
            (prev1.position.z - prev2.position.z)
        };
        
        com_point_t dir_after = {
            (next1.position.x - current.position.x),
            (next1.position.y - current.position.y),
            (next1.position.z - current.position.z)
        };
        
        // 计算方向夹角
        float dot_product = dir_before.x * dir_after.x + 
                           dir_before.y * dir_after.y + 
                           dir_before.z * dir_after.z;
        
        float mag_before = sqrt(dir_before.x * dir_before.x + 
                               dir_before.y * dir_before.y + 
                               dir_before.z * dir_before.z);
        float mag_after = sqrt(dir_after.x * dir_after.x + 
                              dir_after.y * dir_after.y + 
                              dir_after.z * dir_after.z);
        
        float cos_angle = dot_product / (mag_before * mag_after);
        float angle_change = acos(fmax(fmin(cos_angle, 1.0f), -1.0f)) * 180.0f / M_PI;
        
        // 击球点判断条件
        bool is_hit_point = (speed_change > speed_threshold_) && 
                           (angle_change > direction_change_threshold_);
        
        if (is_hit_point) {
            hit_point_t hit;
            hit.frame_idx = current.frame_idx;
            hit.position = current.position;
            hit.velocity_before = prev1.velocity;
            hit.velocity_after = next1.velocity;
            hit.speed_change = speed_change;
            hit.confidence = (speed_change / speed_threshold_) * (angle_change / direction_change_threshold_);
            
            hit_points.push_back(hit);
            
            printf("=== HIT POINT DETECTED ===\n");
            printf("Frame: %d\n", hit.frame_idx);
            printf("Position: (%.3f, %.3f, %.3f)\n", hit.position.x, hit.position.y, hit.position.z);
            printf("Speed change: %.3f m/s\n", speed_change);
            printf("Direction change: %.3f degrees\n", angle_change);
            printf("Confidence: %.3f\n", hit.confidence);
        }
    }
    
    return hit_points;
}
